using System;
using Sce.PlayStation.Core;
namespace Nyanimals
{
	public static class CMath
	{
        public static float GetLamdaAsDecimal(float min, float max, float val)
        {
            float numerator = val - min;
            float denominator = max - min;
            float lamda = numerator / denominator;
            
            return lamda;
        }
        public static int GetLamdaAsPercent(float min, float max, float val)
        {
            float lamda = GetLamdaAsDecimal(min, max, val);
            lamda *= 100;
            return (int)lamda;
        }
		public static float Squared_Distence(ref Vector3 vec1, ref Vector3 vec2)
		{
			float x = vec2.X - vec1.X;
			float y = vec2.Y - vec1.Y;
			float z = vec2.Z - vec1.Z;
			
			x *= x;
			y *= y;
			z *= z;
			
			return x + y + z;
		}
		public static float Squared_Distence(Vector3 vec1, Vector3 vec2)
		{
			return Squared_Distence(ref vec1, ref vec2);
		}
		public static float Distence(Vector3 vec1, Vector3 vec2)
		{
			return FMath.Sqrt(Squared_Distence(vec1, vec2));			
		}
		public static float Dot_Product(Vector3 vec1, Vector3 vec2)
		{
			return (vec1.X * vec2.X) + (vec1.Y * vec2.Y) + (vec1.Z + vec2.Z);
		}
        public static float Interpolate(float f1, float f2, float lam)
        {
            float ret = f1 + (f2 - f1) * lam;
         
            return ret;
        }
        public static Vector3 Interpolate(Vector3 vec1, Vector3 vec2, float lam)
        {
         Vector3 vec = vec1 + (vec2 - vec1) * lam;
         
         return vec;
        }
		public static Matrix4 Interpolate(Matrix4 mat1, Matrix4 mat2, float lam)
		{
            // lerp the position
			Vector3 pos1 = mat1.AxisW;
			Vector3 pos2 = mat2.AxisW;
			Vector3 lerpedPosition = Interpolate(pos1, pos2, lam);
            
            // Slerp the rotation
			Quaternion quat1 = Quaternion.FromMatrix4(mat1);
			Quaternion quat2 = Quaternion.FromMatrix4(mat2);
			Quaternion slerpedQuat = Quaternion.Slerp(quat1, quat2, lam);
			
			// recombine and return
			Matrix4 result = slerpedQuat.ToMatrix4();
			result.AxisW = lerpedPosition;
			
			return result;
		}
		public static void LookAt(ref Matrix4 orient, Vector3 target, Vector3 Up)
		{
			// build orientation
			Vector3 ZAxis = target - orient.AxisW;
			if(ZAxis == Vector3.Zero)
			{
				Console.WriteLine("CMath::LookAt() Failed! target is orient.AxisW");
				return;
			}
			ZAxis = ZAxis.Normalize();
			Vector3 XAxis = Up.Cross(ZAxis);
			XAxis = XAxis.Normalize();
			Vector3 YAxis = ZAxis.Cross(XAxis);
			YAxis.Normalize();
			
			// set it
			orient.AxisX = XAxis;
			orient.AxisY = YAxis;
			orient.AxisZ = ZAxis;
			
		}
		
        #region CONVERTS
        
        
        #endregion
        
		#region COLLISION
		
        // do a basic point in sphere test
        public static bool PointInSphere(CollisionSphere s, Vector3 pt)
        {
            float distS = Squared_Distence(s.Position, pt);
            float diameter = s.Radius * s.Radius;
            
            return distS < diameter;
        }
        // do a basic sphere to sphere test
        public static bool SphereToSphere(CollisionSphere s1, CollisionSphere s2)
        {
            float distS = Squared_Distence(s1.Position, s2.Position);
            float diameter = s1.Radius + s2.Radius;
            diameter *= diameter;
            
            return distS < diameter;
        }
        // do a sphere to sphere test and return a new position to get out of the collition
        // NOTE: the new position is for S1 and will be the DELTA position
        public static bool SphereToSphere(CollisionSphere s1, CollisionSphere s2, out Vector3 newPos)
        {
            float dist = Distence(s1.Position, s2.Position);
            float diameter = s1.Radius + s2.Radius;            
            bool result = dist < diameter;            
            
            // the distence to move back
            float fixDist = diameter - dist;
            
            // direction to push
            Vector3 dir = s2.Position - s1.Position; 
            dir = dir.Normalize();
            
            // handle if the positions are on the same spot
            if(dir == Vector3.Zero)
            {
               dir.Z += 0.5f;
            }
            
            // the new position
            newPos = dir * fixDist;            
            return result;
        }
        
        /// <summary>
        /// Point is on a plane.
        /// </summary>
        /// <returns>
        ///  0 = on plane
        ///  1 = infront
        /// -1 = behind
        /// </returns>
        /// <param name='plane'>
        /// Plane.
        /// </param>
        /// <param name='point'>
        /// Point.
        /// </param>
        static readonly float FLOAT_BUFFER = 0.0001f;
        public static int PointOnPlane(CollisionPlane plane, Vector3 point, out float scalar)
        {
            Vector3 toPoint = point - plane.Position;
            toPoint = toPoint.Normalize();
            
            //scalar = -Dot_Product(plane.Normal, toPoint);
            
            float dist = Distence(plane.Position, Vector3.Zero);
            scalar = Dot_Product(plane.Normal, point) + dist;
                        
            if(scalar > FLOAT_BUFFER)
                return 1;
            if(scalar < -FLOAT_BUFFER)
                return -1;
            return 0;
        }
        public static int PointOnPlane(CollisionPlane plane, Vector3 point)
        {
            float garbage;
            return PointOnPlane(plane, point, out garbage);
        }
        
        
        /// <summary>
        /// Computes a plane. NOTE: Point 1 is plane position
        /// </summary>
        /// <param name='pt1'>
        /// Point 1.
        /// </param>
        /// <param name='pt2'>
        /// Point 2
        /// </param>
        /// <param name='pt3'>
        /// Point 3
        /// </param>
        /// <param name='plane'>
        /// The plane to return
        /// </param>
        public static void ComputePlane(Vector3 pt1, Vector3 pt2, Vector3 pt3, out CollisionPlane plane)
        {
            plane = ComputePlane(pt1, pt2, pt3);
        }
        
        /// <summary>
        /// Computes a plane. NOTE: Point 1 is plane position
        /// </summary>
        /// <returns>
        /// The plane.
        /// </returns>
        /// <param name='pt1'>
        /// Point 1.
        /// </param>
        /// <param name='pt2'>
        /// Point 2
        /// </param>
        /// <param name='pt3'>
        /// Point 3
        /// </param>
        public static CollisionPlane ComputePlane(Vector3 pt1, Vector3 pt2, Vector3 pt3)
        {
            // get 2 vectors to cross
            Vector3 vec1 = pt2 - pt1;
            Vector3 vec2 = pt3 - pt2;
            
            // cross the vectors then normalize
            Vector3 normal = vec1.Cross(pt2);
            CollisionPlane plane = new CollisionPlane(pt1, normal);
            
            // get the offset
            // plane.offset = dot_product(pointA, plane.normal);
            return plane;
        }
        
        /// <summary>
        /// Performs Halfspace Test with a point
        /// </summary>
        /// <returns>
        /// 1 if point is on or in front of plane
        /// 2 if point is behind the plane
        /// </returns>
        /// <param name='plane'>
        /// Plane.
        /// </param>
        /// <param name='point'>
        /// Point.
        /// </param>
        public static int PointToPlane(CollisionPlane plane, Vector3 point)
        {
            float halfSpace = Dot_Product(plane.Normal, point) - plane.Offset;
            
            if(halfSpace >= 0)
            {
                return 1;
            }
            else
            {
                return 2;
            }
        }
        
        /// <summary>
        /// Perform Sphere to plane Test
        /// </summary>
        /// <returns>
        /// 1 if sphere is in front of the plane
        /// 2 if sphere is behind the plane
        /// 3 if sphere overlaps the plane
        /// </returns>
        /// <param name='plane'>
        /// Plane.
        /// </param>
        /// <param name='sphere'>
        /// Sphere.
        /// </param>
        public static int SphereToPlane(CollisionPlane plane, CollisionSphere sphere)
        {
            float dist = Dot_Product(sphere.Position, plane.Normal) - plane.Offset;
        
            if(dist > sphere.Radius)
            {
                return 1;
            }
            else if(dist < -sphere.Radius)
            {
                return 2;
            }
            else
            {
                return 3;
            }      
        
        }
        
        
        
        
        private static CollisionSphere debugSpot = new CollisionSphere(Vector3.Zero, 0.2f);
        private static CollisionPlane[] planes = new CollisionPlane[6] {new CollisionPlane(Vector3.Zero, Vector3.Zero),
        new CollisionPlane(Vector3.Zero, Vector3.Zero),
        new CollisionPlane(Vector3.Zero, Vector3.Zero),
        new CollisionPlane(Vector3.Zero, Vector3.Zero),
        new CollisionPlane(Vector3.Zero, Vector3.Zero),
        new CollisionPlane(Vector3.Zero, Vector3.Zero)};
        private static bool once = false;
        public static bool SphereToBox(CollisionSphere sphere, CollisionBox box)
        {
            // find closest point on the sphere
            Vector3 toBox = box.Position - sphere.Position;
            toBox = toBox.Normalize();
            Vector3 closestPoint = sphere.Position + (toBox * sphere.Radius);
            
            debugSpot.Position = closestPoint;
            
            // check if behind all the sides
            CMesh boxMesh = box.BoxMesh;
            bool ret = true;
            for(int sideIndex = 0; sideIndex < 6; ++sideIndex)
            {
                // build plane for this side of the box
                int triIndex = sideIndex * 6;
                
                // get the plane centroid
                int vert0 = boxMesh.Indices[triIndex + 0] * 3;
                int vert1 = boxMesh.Indices[triIndex + 1] * 3;
                int vert2 = boxMesh.Indices[triIndex + 2] * 3;
                Vector3 pos0 = new Vector3(boxMesh.Positions[vert0 + 0], boxMesh.Positions[vert0 + 1], boxMesh.Positions[vert0 + 2]);
                Vector3 pos1 = new Vector3(boxMesh.Positions[vert1 + 0], boxMesh.Positions[vert1 + 1], boxMesh.Positions[vert1 + 2]);
                Vector3 pos2 = new Vector3(boxMesh.Positions[vert2 + 0], boxMesh.Positions[vert2 + 1], boxMesh.Positions[vert2 + 2]);
                planes[sideIndex].Position = (pos0 + pos1 + pos2) / 3;
                planes[sideIndex].Position = box.Context.WorldMatrix.TransformPoint(planes[sideIndex].Position);
                
                // calculate the plane normal
                Vector3 p0p1 = pos1 - pos0;
                Vector3 p1p2 = pos2 - pos1;
                planes[sideIndex].Normal = Vector3.Cross(p0p1, p1p2).Normalize();
                
                // do the point with plane check
                int result = PointOnPlane(planes[sideIndex], closestPoint);
                
                // if in front then no collision
                if(result < 0)
                {
                    planes[sideIndex].Context.Active = false;
                    ret = false;
                }
                else
                {
                    planes[sideIndex].Context.Active = true;
                }
            }
            
            return ret;
        }
		
		#endregion
		
	}
}









